/*
Copyright (c) [2019年5月1日] [吴超]
[MBT_studio] is licensed under Mulan PSL v2.
You can use this software according to the terms and conditions of the Mulan PSL v2.
You may obtain a copy of Mulan PSL v2 at:
		 http://license.coscl.org.cn/MulanPSL2
THIS SOFTWARE IS PROVIDED ON AN "AS IS" BASIS, WITHOUT WARRANTIES OF ANY KIND,
EITHER EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO NON-INFRINGEMENT,
MERCHANTABILITY OR FIT FOR A PARTICULAR PURPOSE.
See the Mulan PSL v2 for more details.
*/


#pragma once
 
#include <math.h>
#include "基本类型.h"
#include "Vector/trigonometry.h"
//#include "ParentCode/mLib/mglobaldatatype.h"

//矩阵拷贝
inline void Matrix4X4f_Copy_old_n1(const float mat[16], float toMat[16]){ for(uint32 i=0; i<16; ++i) toMat[i] = mat[i]; }
inline void Matrix4X4f_Copy(float toMat[16], const float mat[16]){ for(uint32 i=0; i<16; ++i) toMat[i] = mat[i]; }
inline void Matrix4X4f_Copy_3X3(const float mat[16], float toMat[16]){ for(uint32 i=0; i<12; ++i) toMat[i] = mat[i]; }
inline void Matrix4X4d_Copy(const double mat[16], double toMat[16]){ for(uint32 i=0; i<16; ++i) toMat[i] = mat[i]; }
inline void Matrix4X4d_Copy_3X3(const double mat[16], double toMat[16]){ for(uint32 i=0; i<12; ++i) toMat[i] = mat[i]; }

//填充单位矩阵
inline void Matrix4X4f_Identity(float matA[16]){
	/*matA[0] = 1.0; matA[1] = 0.0; matA[2] = 0.0; matA[3] = 1.0;
	matA[4] = 0.0; matA[5] = 1.0; matA[6] = 0.0; matA[7] = 1.0;
	matA[8] = 0.0; matA[9] = 0.0; matA[10] = 1.0; matA[11] = 1.0;
	matA[12] = 0.0; matA[13] = 0.0; matA[14] = 0.0; matA[15] = 1.0;*/

	matA[0] = 1.0; matA[1] = 0.0; matA[2] = 0.0; matA[3] = 0.0;
	matA[4] = 0.0; matA[5] = 1.0; matA[6] = 0.0; matA[7] = 0.0;
	matA[8] = 0.0; matA[9] = 0.0; matA[10] = 1.0; matA[11] = 0.0;
	matA[12] = 0.0; matA[13] = 0.0; matA[14] = 0.0; matA[15] = 1.0;

}
//填充单位矩阵
inline void Matrix4X4d_Identity(double matA[16]){
	matA[0] = 1.0; matA[1] = 0.0; matA[2] = 0.0; matA[3] = 0.0;
	matA[4] = 0.0; matA[5] = 1.0; matA[6] = 0.0; matA[7] = 0.0;
	matA[8] = 0.0; matA[9] = 0.0; matA[10] = 1.0; matA[11] = 0.0;
	matA[12] = 0.0; matA[13] = 0.0; matA[14] = 0.0; matA[15] = 1.0;
}

//This code comes directly from GLU except that it is for float
int Matrix4X4_InvertMatrixf2(const float *m, float *out);



//4*4矩阵相乘
inline void Matrix4X4_Mult(const float matA[16], const float matB[16], float matResult[16]){
	float tempData[4] = {0.0,0.0,0.0,0.0};

	tempData[0] = matA[0]*matB[0] + matA[4]*matB[1] + matA[8]*matB[2] + matA[12] * matB[3];
	tempData[1] = matA[0]*matB[4] + matA[4]*matB[5] + matA[8]*matB[6] + matA[12] * matB[7];
	tempData[2] = matA[0]*matB[8] + matA[4]*matB[9] + matA[8]*matB[10] + matA[12] * matB[11];
	tempData[3] = matA[0]*matB[12] + matA[4]*matB[13] + matA[8]*matB[14] + matA[12] * matB[15];
	matResult[0] = tempData[0]; matResult[4] = tempData[1]; matResult[8] = tempData[2]; matResult[12] = tempData[3];

	tempData[0] = matA[1]*matB[0] + matA[5]*matB[1] + matA[9]*matB[2] + matA[13] * matB[3];
	tempData[1] = matA[1]*matB[4] + matA[5]*matB[5] + matA[9]*matB[6] + matA[13] * matB[7];
	tempData[2] = matA[1]*matB[8] + matA[5]*matB[9] + matA[9]*matB[10] + matA[13] * matB[11];
	tempData[3] = matA[1]*matB[12] + matA[5]*matB[13] + matA[9]*matB[14] + matA[13] * matB[15];
	matResult[1] = tempData[0]; matResult[5] = tempData[1]; matResult[9] = tempData[2]; matResult[13] = tempData[3];

	tempData[0] = matA[2]*matB[0] + matA[6]*matB[1] + matA[10]*matB[2] + matA[14] * matB[3];
	tempData[1] = matA[2]*matB[4] + matA[6]*matB[5] + matA[10]*matB[6] + matA[14] * matB[7];
	tempData[2] = matA[2]*matB[8] + matA[6]*matB[9] + matA[10]*matB[10] + matA[14] * matB[11];
	tempData[3] = matA[2]*matB[12] + matA[6]*matB[13] + matA[10]*matB[14] + matA[14] * matB[15];
	matResult[2] = tempData[0]; matResult[6] = tempData[1]; matResult[10] = tempData[2]; matResult[14] = tempData[3];

	tempData[0] = matA[3]*matB[0] + matA[7]*matB[1] + matA[11]*matB[2] + matA[15] * matB[3];
	tempData[1] = matA[3]*matB[4] + matA[7]*matB[5] + matA[11]*matB[6] + matA[15] * matB[7];
	tempData[2] = matA[3]*matB[8] + matA[7]*matB[9] + matA[11]*matB[10] + matA[15] * matB[11];
	tempData[3] = matA[3]*matB[12] + matA[7]*matB[13] + matA[11]*matB[14] + matA[15] * matB[15];
	matResult[3] = tempData[1]; matResult[7] = tempData[1]; matResult[11] = tempData[2]; matResult[15] = tempData[3];

}
//4*4矩阵相乘
inline void Matrix4X4_Multf(float matResult[16], const float matA[16], const float matB[16]){
	matResult[0] = matA[0]*matB[0] +  matA[4]*matB[1] +  matA[8]*matB[2] + matA[12]*matB[3];
	matResult[4] = matA[0]*matB[4] +  matA[4]*matB[5] + matA[8]*matB[6] +  matA[12]*matB[7];
	matResult[8] = matA[0]*matB[8] +  matA[4]*matB[9] +  matA[8]*matB[10] + matA[12]*matB[11];
	matResult[12] = matA[0]*matB[12] + matA[4]*matB[13] + matA[8]*matB[14] + matA[12]*matB[15];
	matResult[1] = matA[1]*matB[0] +  matA[5]*matB[1] + matA[9]*matB[2] + matA[13]*matB[3];
	matResult[5] = matA[1]*matB[4] + matA[5]*matB[5]+ matA[9]*matB[6]+ matA[13]*matB[7];
	matResult[9] = matA[1]*matB[8] + matA[5]*matB[9] + matA[9]*matB[10] + matA[13]*matB[11];
	matResult[13] =matA[1]*matB[12] + matA[5]*matB[13] + matA[9]*matB[14] + matA[13]*matB[15];
	matResult[2] = matA[2]*matB[0] + matA[6]*matB[1] + matA[10]*matB[2] + matA[14]*matB[3];
	matResult[6]= matA[2]*matB[4] + matA[6]*matB[5] + matA[10]*matB[6] + matA[14]*matB[7];
	matResult[10] = matA[2]*matB[8] + matA[6]*matB[9] + matA[10]*matB[10] + matA[14]*matB[11];
	matResult[14] = matA[2]*matB[12] + matA[6]*matB[13] + matA[10]*matB[14] + matA[14]*matB[15];
	matResult[3] = matA[3]*matB[0] + matA[7]*matB[1] + matA[11]*matB[2] + matA[15]*matB[3];
	matResult[7] = matA[3]*matB[4] + matA[7]*matB[5] + matA[11]*matB[6] + matA[15]*matB[7];
	matResult[11] = matA[3]*matB[8] + matA[7]*matB[9] + matA[11]*matB[10] + matA[15]*matB[11];
	matResult[15] = matA[3]*matB[12] + matA[7]*matB[13] + matA[11]*matB[14] + matA[15]*matB[15];

	 return;
	float tempData[4] = {0.0,0.0,0.0,0.0};

	tempData[0] = matA[0]*matB[0] + matA[4]*matB[1] + matA[8]*matB[2] + matA[12] * matB[3];
	tempData[1] = matA[0]*matB[4] + matA[4]*matB[5] + matA[8]*matB[6] + matA[12] * matB[7];
	tempData[2] = matA[0]*matB[8] + matA[4]*matB[9] + matA[8]*matB[10] + matA[12] * matB[11];
	tempData[3] = matA[0]*matB[12] + matA[4]*matB[13] + matA[8]*matB[14] + matA[12] * matB[15];
	matResult[0] = tempData[0]; matResult[4] = tempData[1]; matResult[8] = tempData[2]; matResult[12] = tempData[3];

	tempData[0] = matA[1]*matB[0] + matA[5]*matB[1] + matA[9]*matB[2] + matA[13] * matB[3];
	tempData[1] = matA[1]*matB[4] + matA[5]*matB[5] + matA[9]*matB[6] + matA[13] * matB[7];
	tempData[2] = matA[1]*matB[8] + matA[5]*matB[9] + matA[9]*matB[10] + matA[13] * matB[11];
	tempData[3] = matA[1]*matB[12] + matA[5]*matB[13] + matA[9]*matB[14] + matA[13] * matB[15];
	matResult[1] = tempData[0]; matResult[5] = tempData[1]; matResult[9] = tempData[2]; matResult[13] = tempData[3];

	tempData[0] = matA[2]*matB[0] + matA[6]*matB[1] + matA[10]*matB[2] + matA[14] * matB[3];
	tempData[1] = matA[2]*matB[4] + matA[6]*matB[5] + matA[10]*matB[6] + matA[14] * matB[7];
	tempData[2] = matA[2]*matB[8] + matA[6]*matB[9] + matA[10]*matB[10] + matA[14] * matB[11];
	tempData[3] = matA[2]*matB[12] + matA[6]*matB[13] + matA[10]*matB[14] + matA[14] * matB[15];
	matResult[2] = tempData[0]; matResult[6] = tempData[1]; matResult[10] = tempData[2]; matResult[14] = tempData[3];

	tempData[0] = matA[3]*matB[0] + matA[7]*matB[1] + matA[11]*matB[2] + matA[15] * matB[3];
	tempData[1] = matA[3]*matB[4] + matA[7]*matB[5] + matA[11]*matB[6] + matA[15] * matB[7];
	tempData[2] = matA[3]*matB[8] + matA[7]*matB[9] + matA[11]*matB[10] + matA[15] * matB[11];
	tempData[3] = matA[3]*matB[12] + matA[7]*matB[13] + matA[11]*matB[14] + matA[15] * matB[15];
	matResult[3] = tempData[1]; matResult[7] = tempData[1]; matResult[11] = tempData[2]; matResult[15] = tempData[3];

}




//矩阵转置
inline void Matrix4X4_Transpose(const float mat[16], float matResult[16]){
	matResult[0] = mat[0];
	matResult[4] = mat[1];
	matResult[8] = mat[2];
	matResult[12] = mat[3];

	matResult[1] = mat[4];
	matResult[5] = mat[5];
	matResult[9] = mat[6];
	matResult[13] = mat[7];

	matResult[2] = mat[8];
	matResult[6] = mat[19];
	matResult[10] = mat[10];
	matResult[14] = mat[11];

	matResult[3] = mat[12];
	matResult[7] = mat[13];
	matResult[11] = mat[14];
	matResult[15] = mat[15];

}

//构建旋转矩阵
inline void Matrix4X4_BuildTranformMat(float mat[16], const float tranform[3]){
	Matrix4X4f_Identity(mat);
	mat[12] = tranform[0];
	mat[13] = tranform[1];
	mat[14] = tranform[2];
}

//移动
inline void Matrix4X4_Location(float mat[16], const float pos[3]){
	mat[12] = mat[0] * pos[0] + mat[4] * pos[1] + mat[8]  * pos[2] + mat[12];
	mat[13] = mat[1] * pos[0] + mat[5] * pos[1] + mat[9]  * pos[2] + mat[13];
	mat[14] = mat[2] * pos[0] + mat[6] * pos[1] + mat[10] * pos[2] + mat[14];
	mat[15] = mat[3] * pos[0] + mat[7] * pos[1] + mat[11] * pos[2] + mat[15];

}
inline void Matrix4X4_Tranformv(float mat[16], const float pos[3]){
	float tempMatrix[16];
	Matrix4X4f_Identity(tempMatrix);

	Matrix4X4_Location(tempMatrix, pos);
	Matrix4X4_Mult(mat, mat, tempMatrix);

}


inline void Matrix4X4f_addMove(float mat[16], const float pos[3]){
	mat[12] += pos[0];
	mat[13] += pos[1];
	mat[14] += pos[2];
}


inline void Matrix4X4_Tranform(float mat[16], const float x, const float y, const float z){
	/*mat[12] += x;
	mat[13] += y;
	mat[14] += z;*/

	float tempMatrix[16];
	Matrix4X4f_Identity(tempMatrix);
	tempMatrix[12] = x;
	tempMatrix[13] = y;
	tempMatrix[14] = z;
	//把移动矩阵很当前矩阵相乘
	Matrix4X4_Mult(mat, mat, tempMatrix);
}

//构建旋转矩阵
inline void Matrix4X4_BuildRotationMat(float mat[16], const float rot[3]){
	Matrix4X4f_Identity(mat);
	if(rot[0]){
		//mat[5] = cos(radtodu(rot[0])); mat[6] = -sin(radtodu(rot[0]));
		//mat[9] = sin(radtodu(rot[0])); mat[10] = cos(radtodu(rot[0]));
		mat[5] = cos(radtodu(rot[0])); mat[9] = -sin(radtodu(rot[0]));
		mat[6] = sin(radtodu(rot[0])); mat[10] = cos(radtodu(rot[0]));
	}
	if(rot[1]){
		mat[0] = cos(radtodu(rot[1])); mat[2] = sin(radtodu(rot[1]));
		mat[8] = -sin(radtodu(rot[1])); mat[10] = cos(radtodu(rot[1]));
		//mat[0] = cos(rot[1]); mat[2] = sin(rot[1]);
		//mat[8] = -sin(rot[1]); mat[10] = cos(rot[1]);
	}
	if(rot[2]){
		mat[0] = cos(radtodu(rot[2])); mat[1] = -sin(radtodu(rot[2]));
		mat[4] = sin(radtodu(rot[2])); mat[5] = cos(radtodu(rot[2]));
		//mat[0] = cos(rot[2]); mat[1] = -sin(rot[2]);
		//mat[4] = sin(rot[2]); mat[5] = cos(rot[2]);
	}
}
inline void Matrix4X4_BuildRotationXMat(float mat[16], const float x){
	Matrix4X4f_Identity(mat);
	mat[5] = cos(radtodu(x)); mat[6] = -sin(radtodu(x));
	mat[9] = sin(radtodu(x)); mat[10] = cos(radtodu(x));
	//mat[5] = cos(radtodu(x)); mat[9] = -sin(radtodu(x));
	//mat[6] = sin(radtodu(x)); mat[10] = cos(radtodu(x));
}
inline void Matrix4X4_BuildRotationYMat(float mat[16], const float y){
	Matrix4X4f_Identity(mat);
	mat[0] = cos(radtodu(y)); mat[2] = sin(radtodu(y));
	mat[8] = -sin(radtodu(y)); mat[10] = cos(radtodu(y));
	//mat[0] = cos(radtodu(y)); mat[2] = -sin(radtodu(y));
	//mat[8] = sin(radtodu(y)); mat[10] = cos(radtodu(y));
}
inline void Matrix4X4_BuildRotationZMat(float mat[16], const float z){
	Matrix4X4f_Identity(mat);
	mat[0] = cos(radtodu(z)); mat[1] = -sin(radtodu(z));
	mat[4] = sin(radtodu(z)); mat[5] = cos(radtodu(z));
	//mat[0] = cos(radtodu(z)); mat[1] = sin(radtodu(z));
	//mat[4] = -sin(radtodu(z)); mat[5] = cos(radtodu(z));
}


//旋转矩阵
inline void Matrix4X4_Rotation(float mat[16], const float rot[3]){
	float rotMatrix[16];
	//Matrix4X4_BuildRotationMat(rotMatrix, rot);
	//X
	Matrix4X4_BuildRotationXMat(rotMatrix, rot[0]);
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Y
	Matrix4X4_BuildRotationYMat(rotMatrix, rot[1]);
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Z
	Matrix4X4_BuildRotationZMat(rotMatrix, rot[2]);
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Matrix4X4_Mult(mat, rotMatrix, mat);
}
inline void Matrix4X4_Rotation3(float mat[16], const float rot[3], const float mov[3]){
	float rotMatrix[16];
	//Matrix4X4_BuildRotationMat(rotMatrix, rot);
	//X
	Matrix4X4_BuildRotationXMat(rotMatrix, rot[0]);
	rotMatrix[12] = mov[0];
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Y
	Matrix4X4_BuildRotationYMat(rotMatrix, rot[1]);
	rotMatrix[13] = mov[1];
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Z
	Matrix4X4_BuildRotationZMat(rotMatrix, rot[2]);
	rotMatrix[14] = mov[2];
	Matrix4X4_Mult(mat, rotMatrix, mat);
	//Matrix4X4_Mult(mat, rotMatrix, mat);
}




//构建缩放矩阵
inline void Matrix4X4_BuildScale(float mat[16], const float scale[3]){
	Matrix4X4f_Identity(mat);
	mat[0] = scale[0]; mat[5] = scale[1]; mat[10] = scale[2]; mat[15] = 1.0f;
}
inline void Matrix4X4_BuildScaleX(float mat[16], const float scale){
	Matrix4X4f_Identity(mat);
	mat[0] = scale;
}
inline void Matrix4X4_BuildScaleY(float mat[16], const float scale){
	Matrix4X4f_Identity(mat);
	mat[5] = scale;
}
inline void Matrix4X4_BuildScaleZ(float mat[16], const float scale){
	Matrix4X4f_Identity(mat);
	mat[10] = scale;
}

//缩放矩阵
inline void Matrix4X4_Scale(float mat[16], const float scale[3]){
	float rotMatrix[16];
	Matrix4X4_BuildScale(rotMatrix, scale);
	Matrix4X4_Mult(mat, rotMatrix, mat);

	//Matrix4X4_BuildScaleX(rotMatrix, scale[0]);
	//Matrix4X4_Mult(mat, rotMatrix, mat);
	//Matrix4X4_BuildScaleY(rotMatrix, scale[1]);
	//Matrix4X4_Mult(mat, rotMatrix, mat);
	//Matrix4X4_BuildScaleZ(rotMatrix, scale[2]);
	//Matrix4X4_Mult(mat, rotMatrix, mat);
}


//变换向量
inline void Matrix4X4_TranformVec(float toVec[3], const float vec[3], const float mat[16]){
	toVec[0] = vec[0]*mat[0] + vec[1]*mat[4] + vec[2]*mat[8] + mat[12];
	toVec[1] = vec[0]*mat[1] + vec[1]*mat[5] + vec[2]*mat[9] + mat[13];
	toVec[2] = vec[0]*mat[2] + vec[1]*mat[6] + vec[2]*mat[10] + mat[14];

}

inline void Matrix4X4_TranformVec4(float *resultVec, const float *matrix, const float *pvector)
{
	resultVec[0] = matrix[0]*pvector[0] + matrix[4]*pvector[1] + matrix[8]*pvector[2] + matrix[12]*pvector[3];
	resultVec[1] = matrix[1]*pvector[0] + matrix[5]*pvector[1] + matrix[9]*pvector[2] + matrix[13]*pvector[3];
	resultVec[2] = matrix[2]*pvector[0] + matrix[6]*pvector[1] + matrix[10]*pvector[2] + matrix[14]*pvector[3];
	resultVec[3] = matrix[3]*pvector[0] + matrix[7]*pvector[1] + matrix[11]*pvector[2] + matrix[15]*pvector[3];
}








